/**
 *@description: pt-thread.h提供了CHECK系列宏
 *@author: Kalimdorsummer
 *@date: 2020-09-28 17:41:52
*/
#include "pt-thread.h"	// 放在前面
#include "my_utils.h"

#ifndef NULL
    #define NULL    (void *)0
#endif // NULL



#undef memset
#undef memcpy
#undef strstr

#ifdef __CC_ARM
#pragma diag_suppress 546
#endif


//=============================================================================================
//遇到了多次由于疏忽导致memcpy数组越界, 为此写一个强制输入最大长度的参数.
void * memcpy_safe ( void * dst, const void * src, uint32_t count, uint32_t max_len )
{
	MY_UTILS_ASSERT(dst != NULL);
	MY_UTILS_ASSERT(src != NULL);

    if (count > max_len){
        count = max_len;
    }
// 检查是否对齐
#undef UNALIGNED
#define UNALIGNED(X)	((int32_t)X & (sizeof(int32_t) - 1))
	char *dst_ptr = (char *)dst;
	char *src_ptr = (char *)src;
	// 如果都不对齐, 但是凸出的部分一致, 例如都对齐3字节
	if ( UNALIGNED(dst) == UNALIGNED(src) ){

		memcpy(dst_ptr, src_ptr, count);

	}else{	// 目标和源不对齐4字节, 而且凸出的部分还不一样, 只能按字节存储, 如果还调用memcpy可能引起hardfault

		while (count--){
			*dst_ptr++ = *src_ptr++;
		}
	}

	return dst;
#undef UNALIGNED
}

/**
 * The  strnlen()  function  returns the number of characters in the
 * string pointed to by s, excluding the terminating null byte ('\0'),
 * but at most maxlen.  In doing this, strnlen() looks only at the
 * first maxlen characters in the string pointed to by s and never
 * beyond s+maxlen.
 *
 * @param s the string
 * @param maxlen the max size
 * @return the length of string
 */
static uint32_t my_strnlen(const char *s, size_t maxlen)
{
    const char *sc;

    for (sc = s; *sc != '\0' && (size_t)(sc - s) < maxlen; ++sc) /* nothing */
        ;

    return sc - s;
}
/**
 * 带有最大范围的字符串查找, 防止无限往后找造成不正确的结果
 *
 * @param s1 the source string
 * @param s2 the find string
 * @param max_len 最大的检查范围
 * @return the first occurrence of a s2 in s1, or NULL if no found.
 */
char * strstr_safe ( const char * s1, const char * s2, uint32_t max_len )
{
	MY_UTILS_ASSERT(s1 != NULL);
	MY_UTILS_ASSERT(s2 != NULL);

	int l1, l2;

	l2 = my_strnlen ( s2, max_len );

	if ( !l2 ) {
		return ( char * ) s1;
	}

	l1 = my_strnlen ( s1, max_len );

	while ( l1 >= l2 ) {
		l1 --;

		if ( !memcmp ( s1, s2, l2 ) ) {
			return ( char * ) s1;
		}

		s1 ++;
	}

	return NULL;
}

/**
 * This function will set the content of memory to specified value
 *
 * @param s the address of source memory
 * @param c the value shall be set in content
 * @param count the copied length
 *
 * @return the address of source memory
 */
void * memset_safe ( void * s, int c, uint32_t count, uint32_t max_len )
{
	MY_UTILS_ASSERT(s != NULL);

    if (count > max_len){
        count = max_len;
    }
    return memset(s, c, count);
}

/**
 *@description: 将一块内存清零
 *@s: 需要清除的内存起始地址,
 *@count: 需要清除的字节数
 *@return: NA
 *@author: Kalimdorsummer
 *@date: 2020-08-28 16:49:41
*/
void * memclear ( void * dst, uint32_t count )
{
	MY_UTILS_ASSERT(dst != NULL);
#if 0   // 若ARMCC开启microlib, memset效率较低, 下面的高一些
    uint32_t u8_cnt = 4 - ((uint32_t)dst & 0x03);    // 看前端是否四字节对齐
    u8_cnt &= 0x03;
    uint32_t u32_cnt = (count - u8_cnt) >> 2;   // 按32位清除的循环数
    uint16_t u8_left = (count - u8_cnt) & 0x3;
    int8_t *u8_xs = dst;
    while (u8_cnt--){
        *u8_xs++ = 0x00;
	}

	int32_t *u32_xs = (int32_t *)u8_xs;
	while (u32_cnt--){
        *u32_xs++ = 0x00000000;
	}

	// 尾部剩余清零
	u8_xs = (int8_t *)u32_xs;
	while (u8_left--){
        *u8_xs++ = 0x00;
	}

	return dst;
#endif // 0
    return memset(dst, 0, count);
}

// 在指定范围内查找目标值, 若找不到返回end
void *cfind_$(const void *start, const void *end, const void *target, int target_size)
{
    char *p;
    for( p = (char *)start; p <= (char *)end; p += target_size){
        if(memcmp(p, target, target_size) == 0){
            break;
        }
    }

    return (void*)p;
}


void *cfind_if_$(const void *start, const void *end, find_cmpfunc_t cmp, size_t target_size)
{
    char *p;
    for( p = (char *)start; p <= (char *)end; p += target_size){
        if(cmp(p)){
            break;
        }

    }

    return (void*)p;
}
//================== 转换操作 ========================
#include <ctype.h>

static inline unsigned char atohex(char val)
{
    if (!isxdigit(val)){
        // 打log, 不是十六进制数
        return 0xFF;
    }
	if ( isdigit(val) ) {         // 高位是数字
		return (val - '0' + 0x0);

	} else if ( isupper(val) ) { //大写字母
		return (val - 'A' + 0xA);

	} else if ( islower(val) ) { //小写字母
		return (val - 'a' + 0xA);
	}

	return 0xFF;
}
//函 数 名：AscToHex()
//功能描述：把ASCII转换为16进制, 如果存在非16进制字符则返回FF, 但是无法区分当hl为FF的情况
unsigned char AscToHex ( char h, char l )
{
    int res_h = atohex(h);
    int res_l = atohex(l);

	return (unsigned char)( res_h << 4 | res_l);
}
/*
 *@description: 将形如 "AB12CD34"的字符串转换成hex 0XAB12CD34
 *@p_hex: 转换后存储的起始地址
 *@p_str: 要转换的字符串地址
 *@len:   要转换的长度, 是str的字节数,
 *@hex_len: 最大字节数, 通常填写p_hex区的大小, 防止越界, 正常str的长度是hex的两倍
 *@return: 错误代码, 0:OK, 1: 有错误字符, 2:p_hex过小,
 *@author: Kalimdorsummer
 *@date: 2020-09-07 9:37:41
*/
int StrToHex ( char * p_hex, char * p_str, int len, int hex_len )
{
	char h, l;
	int i;

    if (hex_len < len / 2){
        return 2;
    }

	for ( i = 0; i < len / 2; i++ ) {

		h = p_str[2 * i];
		if ( !isxdigit(h) ){
            return 1;
		}
		l = p_str[2 * i + 1];
		if ( !isxdigit(l) ){
            return 1;
		}

        p_hex[i] = AscToHex(h, l);
	}

	return 0;
}

int HexToChar ( unsigned char * p_char, unsigned char * p_hex, int len, int char_len )
{
    if (char_len < 2 * len){
        return 2;
    }

    const char char_arr[17] = "0123456789ABCDEF";
    for(int i = 0; i < len; i++)
    {
        p_char[2 * i] = char_arr[p_hex[i] >> 4];
        p_char[2 * i + 1] = char_arr[p_hex[i] & 0x0F];
    }

    return 0;
}
/*
 *@description: 将0x1245ABCD的值转换成字符串"0x1245ABCD"
 *@p_hex: 要转换的内容地址
 *@p_str: 要存储的字符串地址
 *@len:   要转换的长度, 是p_hex的字节数,
 *@hex_len: 最大字节数, 通常填写p_str区的大小, 防止越界, 正常str的长度是hex的两倍
 *@return: 错误代码, 0:OK, 2:p_str过小,
 *@author: Kalimdorsummer
 *@date: 2020-09-07 10:09:12
*/
int HexToStr ( unsigned char * p_str, unsigned char * p_hex, int len, int str_len )
{
    if (str_len < 2 * len + 1){ // 还要留出存储'\0'的单元
        return 2;
    }

    int res;
    res = HexToChar(p_str, p_hex, len, str_len - 1);
    if (res == 0){
        p_str[len * 2] = '\0';
        return 0;
    }

    return res;
}

//================== 转换操作 end ========================

//================== 字节序转换操作 ======================

static inline uint16_t bswap_16(uint16_t x)
{
    uint8_t *p_tmp = (uint8_t *)&x;
    SWAP(p_tmp[0], p_tmp[1]);
    return x;
}

static inline uint32_t bswap_32(uint32_t x)
{
    uint8_t *p_tmp = (uint8_t *)&x;
    SWAP(p_tmp[0], p_tmp[3]);
    SWAP(p_tmp[1], p_tmp[2]);
    return x;
}


static inline uint64_t bswap_64(uint64_t x)
{
    uint8_t *p_tmp = (uint8_t *)&x;
    SWAP(p_tmp[0], p_tmp[7]);
    SWAP(p_tmp[1], p_tmp[6]);
    SWAP(p_tmp[2], p_tmp[5]);
    SWAP(p_tmp[3], p_tmp[4]);
    return x;
}

void gen_bswap(void *p_val, size_t size)
{
    if (!is_even(size)){
        return;
    }

    if (p_val == NULL){
        return;
    }

    uint8_t *p_tmp = (uint8_t *)p_val;
    for (size_t i = 0; i < size / 2; i++){
        SWAP(p_tmp[i], p_tmp[size - 1 - i]);
    }
}

/**
 *@description: 大小端转换
 *@val: 要转换的值
 *@num: 要转换的字节数, 必须是偶数
 *@return: 转换结果
 *@author: Kalimdorsummer
 *@date: 2020-08-18 15:16:37
*/
uint32_t reverse_byte ( uint32_t val, uint8_t num )
{
//  在keil中可以用, gcc不行不知道为什么
//	uint32_t r = 0;
//	char * c = ( char * ) &val;
//
//	for ( int i = 0; i < num; i++ ) {
//		r |= ( * ( c + i ) << ( ( ( num - 1 ) * 8 ) - 8 * i ) );
//	}
//
//	return r;

    switch(num){
        case 2:
            return bswap_16(val);
        case 4:
            return bswap_32(val);
        case 8:
            return bswap_64(val);
        default:
            return val;
    }


}
//================== 字节序转换操作 end ======================

//================= 栈操作 ========================
#ifdef USING_STACK
// 结构体不暴露到头文件, 所有操作需要通过接口函数, 防止意外修改
ALIGN(4)
typedef struct{
    size_t      element_cnt;    // 栈元素个数
    size_t      stack_size;     // 栈大小
    size_t      element_size;   // 栈元素大小
    uint8_t     contents[];     // 栈缓存区内存

}ATTR_PACK stack_adt_t;

STATIC_ASSERT(sizeof(stack_adt_t) == STACK_STRUCT_SIZE, "file:" __FILE__ " line:" TO_STRING(__LINE__));

stack_t stack_create(size_t element_max, size_t element_size)
{
    CHECK_LOCAL(element_max != 0 && element_size != 0, param_null_err);

    stack_adt_t *  p_s;
    p_s = my_malloc( sizeof(stack_adt_t) + element_max * element_size );         // 分配栈实例内存和栈缓存区内存

    CHECK_LOCAL(p_s != NULL, create_failed, "stack create failed\n");


    p_s->element_size = element_size;
    p_s->element_cnt = 0;
    p_s->stack_size = element_max * element_size;
    return (stack_t)p_s;


param_null_err:
create_failed:
    return NULL;
}

void stack_init(stack_t pv_s, size_t element_max, size_t element_size)
{
    CHECK_LOCAL(pv_s != NULL, param_null_err);
    CHECK_LOCAL(element_max != 0 && element_size != 0, param_null_err);

    stack_adt_t *  p_s = (stack_adt_t * )pv_s;
    p_s->element_size = element_size;
    p_s->element_cnt = 0;
    p_s->stack_size = element_max * element_size;

param_null_err:
    return;
}

void stack_destroy(stack_t pv_s)
{
    stack_adt_t * s = (stack_adt_t * )pv_s;
    if(s == NULL){
        return;
    }
    my_free(s);
}

// 栈清空
void stack_make_empty(stack_t pv_s)
{
    stack_adt_t * s = (stack_adt_t * )pv_s;
	if (s == NULL){
		return;
	}

    s->element_cnt = 0;
}

// 检查栈是否空
bool stack_is_empty(stack_t pv_s)
{
    stack_adt_t * s = (stack_adt_t * )pv_s;
    CHECK_LOCAL(s != NULL, param_null_err);
    return s->element_cnt == 0;

param_null_err:
    return false;
}

bool stack_is_full(stack_t pv_s)
{
    stack_adt_t * s = (stack_adt_t * )pv_s;
    CHECK_LOCAL(s != NULL, param_null_err);

    return s->element_cnt * s->element_size  == s->stack_size;

param_null_err:
    return false;
}

// 数据压栈
int32_t stack_push(stack_t pv_s, void *in)
{

    CHECK_LOCAL(pv_s != NULL, param_null_err);
    CHECK_LOCAL(in != NULL, param_null_err);


    if( stack_is_full(pv_s) ){
        return STACK_FULL;
    }
	stack_adt_t * s = (stack_adt_t * )pv_s;
	CHECK_LOCAL( 0 != s->stack_size, stack_err);

    void *buf = &s->contents[s->element_cnt * s->element_size];  // 获取要存入的栈缓存地址
    memcpy(buf, in, s->element_size);
    s->element_cnt++;

    return STACK_STATUS_OK;
param_null_err:
    return STACK_PARAM_NULL;
stack_err:
    return STACK_STATUS_ERR;
}

int32_t stack_pop(stack_t pv_s, void *out)
{

    CHECK_LOCAL(pv_s != NULL, param_null_err);
    CHECK_LOCAL(out != NULL, param_null_err);

    if ( stack_is_empty(pv_s) ){
        return STACK_EMPTY;
    }

	stack_adt_t * s = (stack_adt_t * )pv_s;
	CHECK_LOCAL( 0 != s->stack_size, stack_err);

    void *buf = &s->contents[(--s->element_cnt) * s->element_size];  // 获取要弹出的元素地址
    memcpy(out, buf, s->element_size);

    return STACK_STATUS_OK;

param_null_err:
    return STACK_PARAM_NULL;
stack_err:
    return STACK_STATUS_ERR;
}


size_t stack_max_element(stack_t pv_s)
{
    stack_adt_t * s = (stack_adt_t * )pv_s;
    return (s->stack_size / s->element_size);
}

#endif
//================= 栈操作 end ========================

//================= fifo 操作 =========================
#ifdef USING_FIFO
ALIGN(4)
typedef struct {
 	size_t      fifo_size;
	size_t      put_index;
	size_t      get_index;
	size_t      element_max;
	size_t      element_size;
	size_t      element_cnt;

    size_t      fifo_errno;
	size_t      fifo_status;

	uint8_t     buffer[];
}ATTR_PACK  fifo_adt_t;

STATIC_ASSERT(sizeof(fifo_adt_t) == FIFO_STRUCT_SIZE, "file:" __FILE__ " line:" TO_STRING(__LINE__));

int32_t fifo_init(fifo_t pv_fifo, size_t element_max, size_t element_size)
{
    fifo_adt_t*  p_fifo = (fifo_adt_t* )pv_fifo;

    CHECK_LOCAL(p_fifo          != NULL, param_invalid);
    CHECK_LOCAL(element_max     != 0, param_invalid);
    CHECK_LOCAL(element_size    != 0, param_invalid);

    // 初始化fifo
    p_fifo->element_size      = element_size;
    p_fifo->element_cnt       = 0;
    p_fifo->put_index         = 0;
    p_fifo->get_index         = 0;
    p_fifo->element_max       = element_max;
    p_fifo->fifo_size         = element_max * element_size;
    p_fifo->fifo_errno        = 0;
    p_fifo->fifo_status       = FIFO_EMPTY;

    return FIFO_STATUS_OK;

param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return FIFO_PARAM_INVAL;
}

fifo_t fifo_create(size_t element_max, size_t element_size)
{

    CHECK_LOCAL(element_max != 0, param_null_err);
    CHECK_LOCAL(element_size != 0, param_null_err);

    fifo_t p_fifo;
    p_fifo = (fifo_t)my_malloc( sizeof(fifo_adt_t) + element_max * element_size );         // 分配fifo实例内存和fifo缓存区内存

    CHECK_LOCAL(p_fifo != NULL, create_failed, "fifo create failed\n");

    // 创建成功, 初始化fifo
    fifo_init(p_fifo, element_max, element_size);

    return p_fifo;

param_null_err:
create_failed:
    return NULL;

}

void fifo_destroy(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if(p_fifo == NULL){
        return;
    }
    my_free(p_fifo);
}


uint32_t get_fifo_element_cnt(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        return p_fifo->element_cnt;
    }
    return 0;
}

uint32_t get_fifo_status(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        return p_fifo->fifo_status;
    }
    return 0;
}

uint16_t get_fifo_errno(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        return p_fifo->fifo_errno;
    }
    return 0;
}

void clr_fifo_errno(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        p_fifo->fifo_errno = 0;
    }
}

uint16_t get_clr_fifo_errno(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        uint16_t ret_errno = p_fifo->fifo_errno;
        p_fifo->fifo_errno = 0;
        return ret_errno;
    }
    return 0;
}

/**
 *@description: 向fifo中强行写入一个元素, 满了会覆盖最旧的值
 *@pv_fifo: fifo指针
 *@in_val: 要写入的元素的指针
 *@return: 实际写入的元素个数
 *@author: Kalimdorsummer
 *@date: 2020年9月26日-上午9:30:21
*/
size_t fifo_putf(fifo_t pv_fifo, const void * in_val)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    const void *in = in_val;

    CHECK_LOCAL(p_fifo != NULL,     pv_fifo_null);
    CHECK_LOCAL(in_val != NULL,     param_invalid);
    // 若出现fifo大小不等于元素个数乘以元素大小的情况, 那么就是中途被意外修改了或者没有采用正确的创建fifo的方式
    CHECK_LOCAL( p_fifo->fifo_size == p_fifo->element_max * p_fifo->element_size, fifo_state_err);
    CHECK_LOCAL( p_fifo->fifo_size != 0, fifo_state_err);   // 防止没有初始化

    void *buf; buf = &p_fifo->buffer[p_fifo->put_index];

    // 尾部剩余数组空间还足够放下至少一个元素
    if (p_fifo->fifo_size - p_fifo->put_index >= p_fifo->element_size){
        memcpy(buf, in, p_fifo->element_size);

    }else {
        // 不应该出现末端不足一个元素的情况, 除非中途被修改了元素大小或者fifo大小
        // 要么能正好放下一个元素, 要么就回绕到0
        return 0;
    }

    p_fifo->element_cnt++;
    p_fifo->fifo_status = FIFO_HALF;  // 任意添加一个元素即可不空
    if (p_fifo->element_cnt >= p_fifo->element_max){
        p_fifo->element_cnt = p_fifo->element_max;
        p_fifo->fifo_status = FIFO_FULL;
    }

    p_fifo->put_index += p_fifo->element_size;
    if (p_fifo->put_index >= p_fifo->fifo_size){
        int ov_size = p_fifo->put_index - p_fifo->fifo_size;   // 溢出的字节数
        p_fifo->put_index = ov_size;
    }

    // FIFO满了, 将读指针改写到和写指针一样
    if (p_fifo->fifo_status == FIFO_FULL){
        p_fifo->get_index = p_fifo->put_index;	//丢弃未读的值
    }

    return 1;

param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return 0;
pv_fifo_null:
    return 0;

fifo_state_err:
    p_fifo->fifo_errno = FIFO_STATUS_ERR;
    return 0;

}

// 与putf区别: 满了不会覆盖旧值
size_t fifo_put(fifo_t pv_fifo, const void * in_val)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    CHECK_LOCAL(p_fifo != NULL,     pv_fifo_null);
    CHECK_LOCAL(in_val != NULL,     param_invalid);
    CHECK_LOCAL(p_fifo->fifo_status != FIFO_FULL, fifo_full);

    return fifo_putf(pv_fifo, in_val);

fifo_full:
    return 0;
param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return 0;

pv_fifo_null:
    return 0;
}

static size_t _fifo_write(fifo_t pv_fifo, void const * p_data_in, size_t cnt, bool force_write)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    uint8_t const *p_in = (uint8_t const *)p_data_in;

    CHECK_LOCAL(p_fifo != NULL,     pv_fifo_null);
    CHECK_LOCAL(p_data_in != NULL,  param_invalid);
    CHECK_LOCAL(cnt > 0,            param_invalid);
    CHECK_LOCAL(p_fifo->fifo_size == p_fifo->element_max * p_fifo->element_size, fifo_state_err);
    CHECK_LOCAL( p_fifo->fifo_size != 0, fifo_state_err);

	//写入fifo
	size_t act_write; act_write = 0;
	for(size_t i = 0; i < cnt; i++, p_in += p_fifo->element_size ){
        size_t act_put = 0;
        // 是否强制写入
        if (!force_write){
            act_put = fifo_put((fifo_t)p_fifo, p_in);
            act_write += act_put;
            if (act_put == 0){
                return act_write;
            }
        }else{
            act_put = fifo_putf((fifo_t)p_fifo, p_in);
            act_write += act_put;
        }


	}

    return act_write;

param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return 0;

pv_fifo_null:
    return 0;

fifo_state_err:
    p_fifo->fifo_errno = FIFO_STATUS_ERR;
    return 0;
}
/**
 *@description: 向fifo中写入一定数量的值, 如果满了则覆盖旧值
 *@p_fifo: 要读取的fifo的指针
 *@p_data_in: 要写入的缓存
 *@cnt: 要写入的数量
 *@return: 实际写入的数量
 *@author: Kalimdorsummer
 *@date: 2020年9月21日-17:52:47
*/
size_t fifo_writef(fifo_t pv_fifo, void const * p_data_in, size_t cnt)
{
    return _fifo_write(pv_fifo, p_data_in, cnt, true);
}

// 满了之后不会覆盖旧值
size_t fifo_write(fifo_t pv_fifo, void const * p_data_in, size_t cnt)
{
    return _fifo_write(pv_fifo, p_data_in, cnt, false);
}

size_t fifo_get ( fifo_t pv_fifo, void * out_val )
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    uint8_t *out = (uint8_t *)out_val;

    CHECK_LOCAL(p_fifo != NULL, pv_fifo_null);
    CHECK_LOCAL(out != NULL, param_invalid);
    CHECK_LOCAL(p_fifo->fifo_status != FIFO_EMPTY, fifo_empty_info);
    CHECK_LOCAL(p_fifo->fifo_size == p_fifo->element_max * p_fifo->element_size, fifo_state_err);
    CHECK_LOCAL(p_fifo->fifo_size != 0, fifo_state_err);

	void *buf; buf = &p_fifo->buffer[p_fifo->get_index];
	// 尾部剩余数组空间还足够放下至少一个元素
    if (p_fifo->fifo_size - p_fifo->get_index >= p_fifo->element_size){
        memcpy(out, buf, p_fifo->element_size);

    }else{
        p_fifo->fifo_errno = FIFO_STATUS_ERR;
        return 0;
    }

    p_fifo->element_cnt--;
    p_fifo->fifo_status = FIFO_HALF;  // 任意读出一个元素, 即可不满
    if (p_fifo->element_cnt <= 0){
        p_fifo->element_cnt = 0;
        p_fifo->fifo_status = FIFO_EMPTY;
    }

	p_fifo->get_index += p_fifo->element_size;

	if ( p_fifo->get_index >= p_fifo->fifo_size ) {
        int ov_size = p_fifo->get_index - p_fifo->fifo_size;   // 溢出的字节数
        p_fifo->get_index = ov_size;
	}

	return 1;

pv_fifo_null:
    return 0;
param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return 0;

fifo_empty_info:
    return 0;

fifo_state_err:
    p_fifo->fifo_errno = FIFO_STATUS_ERR;
    return 0;
}

/**
 *@description: 从fifo中读出一定数量的值
 *@p_fifo: 要读取的fifo的指针
 *@p_data_out: 要读出到的目标地址
 *@cnt: 要读取的数量
 *@return: 实际读取的数量
 *@author: Kalimdorsummer
 *@date: 2020年9月21日-17:38:18
*/
size_t fifo_read( fifo_t pv_fifo, void * p_data_out, size_t cnt)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    uint8_t *p_out = (uint8_t *)p_data_out;

    CHECK_LOCAL(p_fifo != NULL,     pv_fifo_null);
    CHECK_LOCAL(p_out != NULL,      param_invalid);
    CHECK_LOCAL(cnt > 0,            param_invalid);

    CHECK_LOCAL(p_fifo->fifo_status != FIFO_EMPTY,                            fifo_empty_info);
    CHECK_LOCAL(p_fifo->fifo_size == p_fifo->element_max * p_fifo->element_size,  fifo_state_err);
    CHECK_LOCAL( p_fifo->fifo_size != 0, fifo_state_err);

    size_t get_count; get_count   = get_fifo_element_cnt((fifo_t)p_fifo);
    size_t req_count; req_count   = cnt;
    size_t read_count; read_count = MIN(get_count, req_count);

    CHECK_LOCAL(read_count != 0, fifo_empty_info);

    size_t act_read; act_read = 0;
	for(size_t i = 0; i < read_count; i++, p_out += p_fifo->element_size ){
		act_read += fifo_get((fifo_t)p_fifo, p_out);
	}


    return act_read;

pv_fifo_null:
    return 0;
param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return 0;

fifo_empty_info:
    return 0;

fifo_state_err:
    p_fifo->fifo_errno = FIFO_STATUS_ERR;
    return 0;
}

// 冲刷fifo
void fifo_flush(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    if (p_fifo != NULL){
        p_fifo->fifo_status = FIFO_EMPTY;
        p_fifo->element_cnt = 0;
        p_fifo->get_index = p_fifo->put_index;
    }

}
/**
 *@description: 获取一个元素但不是完全取出
 *@pv_fifo: 要操作的fifo指针
 *@index: 要获取的元素序号
 *@out: 要输出到的目标地址
 *@return: NA
 *@author: Kalimdorsummer
 *@date: 2020-09-28 11:00:41
*/
void fifo_peek(fifo_t pv_fifo, size_t index, void * out)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    uint8_t *p_out = (uint8_t *)out;

    CHECK_LOCAL(p_fifo != NULL,     					pv_fifo_null);
    CHECK_LOCAL(p_out != NULL,      					param_invalid);
    CHECK_LOCAL(index < p_fifo->element_cnt,            param_invalid);

	size_t ele_size;    ele_size = p_fifo->element_size;
	size_t element_max; element_max = p_fifo->element_max;
	size_t get_index_cnt; get_index_cnt = p_fifo->get_index / ele_size;
	void *src;
	// 如果读取的超出末尾, 要回绕到开头
	if (index + get_index_cnt >= element_max){
        index = index + get_index_cnt - element_max;
        src = &p_fifo->buffer[index * ele_size];	// 当前的读序号加上要读取的序号
	}else{
        src = &p_fifo->buffer[(get_index_cnt + index) * ele_size];	// 当前的读序号加上要读取的序号
	}

	memcpy(p_out, src, ele_size);


pv_fifo_null:
    return;
param_invalid:
    p_fifo->fifo_errno = FIFO_PARAM_INVAL;
    return;
}

size_t fifo_max_element(fifo_t pv_fifo)
{
    fifo_adt_t* p_fifo = (fifo_adt_t* )pv_fifo;
    return p_fifo->element_max;
}

#endif
//================= fifo 操作 end =========================

//================= array 操作 ============================

size_t array_size(void * pv_array)
{
    struct array_common_s *p_array = (struct array_common_s *)pv_array;
    if(p_array == NULL){
        ARRAY_PRINTF("[warning]:[%s]  p_array NULL\n", __FUNCTION__); \
        return 0;
    }

    return p_array->size;
}

void *array_at_$(void *pv_array, size_t idx, size_t element_size)
{
    struct array_common_s *p_array = (struct array_common_s *)pv_array;

    if(p_array != NULL){
        if(idx < p_array->size){
            return (&p_array->array_buffer[idx * element_size]);
        }
    }
    return (void*)0;
}

void array_fill_$(void *pv_array, void *fill_val, size_t element_size)
{
    struct array_common_s *p_array = (struct array_common_s *)pv_array;

    if(pv_array != NULL){
        size_t cnt = p_array->size;
        for(size_t i = 0; i < cnt; i++){
            memcpy(&p_array->array_buffer[i * element_size], fill_val, element_size);
        }
    }
}

void array_init_$(void *pv_array, size_t element_max)
{
    struct array_common_s *p_array = (struct array_common_s *)pv_array;

    if(p_array != NULL){
        p_array->size = element_max;
        p_array->ctn_type = container_type_array;
    }
}

void *array_create_$(size_t element_max, size_t element_size)
{
    struct array_common_s *p_array = (struct array_common_s *)my_malloc( sizeof(struct array_common_s) + element_max * element_size);

    if(p_array != NULL){
        p_array->size = element_max;
    }

    return (void*)p_array;
}


void array_destroy_$(void *pv_array)
{
    if(pv_array != NULL){
        my_free( pv_array );
    }
}

//--------------- 二维数组 ----------------------------

void *arr2d_create_$(size_t row, size_t col, size_t element_size)
{
    struct arr2d_common_s *p_array = (struct arr2d_common_s *)my_malloc( sizeof(struct arr2d_common_s) + row * col * element_size);

    if(p_array != NULL){
        p_array->row = row;
        p_array->col = col;
    }

    return (void*)p_array;
}

void arr2d_destroy_$(void *pv_array)
{
    if(pv_array != NULL){
        my_free( pv_array );
    }
}

size_t arr2d_row(void * pv_array)
{
    struct arr2d_common_s *p_array = (struct arr2d_common_s *)pv_array;
    if(p_array == NULL){
        ARRAY_PRINTF("[warning]:[%s]  p_array NULL\n", __FUNCTION__); \
        return 0;
    }

    return p_array->row;
}

size_t arr2d_col(void * pv_array)
{
    struct arr2d_common_s *p_array = (struct arr2d_common_s *)pv_array;
    if(p_array == NULL){
        ARRAY_PRINTF("[warning]:[%s]  p_array NULL\n", __FUNCTION__); \
        return 0;
    }

    return p_array->col;
}


void *arr2d_at_$(void *pv_array, size_t row, size_t col, size_t element_size)
{
    struct arr2d_common_s *p_array = (struct arr2d_common_s *)pv_array;

    if(p_array != NULL){
        if(row < p_array->row && col < p_array->col){
            return (&p_array->array_buffer[row * element_size][col * element_size]);
        }
    }
    return (void*)0;
}

void arr2d_fill_$(void *pv_array, void *fill_val, size_t element_size)
{
    struct arr2d_common_s *p_array = (struct arr2d_common_s *)pv_array;

    if(pv_array != NULL){
        for(size_t i = 0; i < p_array->row; i++){
            for(size_t j = 0; j < p_array->col; j++){
                memcpy(&p_array->array_buffer[i * element_size][j * element_size], fill_val, element_size);
            }
        }
    }
}
//================= array 操作 end ========================


//================= vector 操作 ===========================
size_t vec_size(void * pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);

    return p_vec->size;
}

size_t vec_capacity(void * pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    return p_vec->capacity;
}

void vec_push_back_$(void *pv_vec, void *in_val, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    if( vec_capacity(p_vec) <= vec_size(p_vec) ){
        char *p_tmp = my_realloc(p_vec->vector_buffer, (vec_capacity(p_vec) + 8) * element_size );

        if (p_tmp == NULL){
            PT_LOG_CALLER_ADDR0();
            pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
        }

        p_vec->vector_buffer = p_tmp;
        p_vec->capacity += 8;
    }

    memcpy_safe(&p_vec->vector_buffer[vec_size(p_vec) * element_size], in_val, element_size, element_size);
    p_vec->size++;
}

void vec_pop_back_$(void *pv_vec, void *out_val, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    if(vec_size(p_vec) > 0){
        memcpy_safe(out_val, &p_vec->vector_buffer[(vec_size(p_vec) - 1) * element_size], element_size, element_size);
        p_vec->size--;
        return;
    }

    memset_safe(out_val, 0, element_size, element_size);    // 如果没有数据就擦除掉
}

void vec_reserve_$(void *pv_vec, size_t new_cap, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);

    if(new_cap <= p_vec->capacity){
        return;
    }

    char *p_tmp = my_realloc(p_vec->vector_buffer, new_cap * element_size );
    if (p_tmp == NULL){
        PT_LOG_CALLER_ADDR0();
        pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
    }

    p_vec->vector_buffer = p_tmp;
    p_vec->capacity = new_cap;
}

void vec_resize_$(void *pv_vec, size_t new_size, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    // 如果new_size小于容量, 只要改size即可
    if(new_size <= p_vec->capacity){
        p_vec->size = new_size;
        return;
    }

    char *p_tmp = my_realloc(p_vec->vector_buffer, new_size * element_size );
    if (p_tmp == NULL){
        PT_LOG_CALLER_ADDR0();
        pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
    }

    p_vec->vector_buffer = p_tmp;
    p_vec->capacity = new_size;

    p_vec->size = new_size;

}

void vec_clear(void *pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    p_vec->size = 0;
}

void *vec_at_$(void *pv_vec, size_t idx, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    if (idx >= p_vec->size){
        PT_LOG_CALLER_ADDR0();
        pt_assert_handler(TO_STRING(idx >= p_vec->size), __FUNCTION__, __LINE__);
    }
    void * ret_val;
    ret_val = &(p_vec->vector_buffer[idx * element_size]);

    return ret_val;
}

void vec_shrink_to_fit_$(void *pv_vec, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);



    if( vec_capacity(p_vec) > vec_size(p_vec) ){
        char *p_tmp;
        // 之前用非堆区的值进行初始化, 这里就要用malloc分配, 原来的非堆区内存忽略
        if (p_vec->not_heap){
            // 没有数据, 直接将buffer置NULL
            if(vec_size( p_vec ) == 0){
                p_vec->vector_buffer = NULL;
                p_vec->capacity = p_vec->size;
                return;
            }
            p_tmp = my_malloc(p_vec->size * element_size);
            if (p_tmp == NULL){
                PT_LOG_CALLER_ADDR0();
                pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
            }

            // 将原来非堆区的值拷贝到新分配的堆中
            memcpy_safe(p_tmp, p_vec->vector_buffer, p_vec->size * element_size, p_vec->size * element_size);
        }else{
            p_tmp = my_realloc(p_vec->vector_buffer, p_vec->size * element_size );
            MY_UTILS_ASSERT(p_tmp != NULL);

            if (p_tmp == NULL){
                PT_LOG_CALLER_ADDR0();
                pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
            }

        }

        p_vec->vector_buffer = p_tmp;   // 缩小分配vector_buffer不会变的
        p_vec->capacity = p_vec->size;

    }
}

void * vec_insert_$(void *pv_vec, void *itor, void *in_val, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    MY_UTILS_ASSERT( (size_t)itor >= (size_t)&p_vec->vector_buffer[0] && (size_t)itor <= (size_t)&p_vec->vector_buffer[element_size * vec_size(p_vec)] );

    vec_resize_$(pv_vec, p_vec->size + 1, element_size);    // 尾部预留一个元素空间

    void *next_itor = (void*)( (size_t)itor + element_size);
    size_t move_cnt = vec_size(p_vec) - ((size_t)itor - (size_t)vec_begin(p_vec)) / element_size;
    memmove(next_itor, itor, move_cnt * element_size);
    memcpy_safe(itor, in_val, element_size, element_size);

    return itor;

}

void * vec_erase_$(void *pv_vec, void *itor, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    MY_UTILS_ASSERT( (size_t)itor >= (size_t)&p_vec->vector_buffer[0] && (size_t)itor < (size_t)&p_vec->vector_buffer[element_size * vec_size(p_vec)] );
    void *next_itor = (void*)( (size_t)itor + element_size);
    size_t move_cnt = vec_size(p_vec) - ((size_t)itor - (size_t)vec_begin(p_vec)) / element_size;
    memmove(itor, next_itor, move_cnt * element_size);
    p_vec->size--;

    return itor;

}


bool vec_empty_$(void *pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);

    return (p_vec->size == 0);
}


void vec_reverse_$(void *pv_vec, void *vbegin, void *vend, size_t element_size)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    MY_UTILS_ASSERT( (size_t)vbegin >= (size_t)&p_vec->vector_buffer[0] && (size_t)vbegin <= (size_t)&p_vec->vector_buffer[element_size * vec_size(p_vec)] );
    MY_UTILS_ASSERT( (size_t)vend >= (size_t)&p_vec->vector_buffer[0] && (size_t)vend <= (size_t)&p_vec->vector_buffer[element_size * vec_size(p_vec)] );
    char *begin = (char *)vbegin;
    char *end = (char *)((size_t)vend - element_size);
    unsigned char temp_in_SWAP_99percent_unique[element_size];

    while(begin < end) {
        memcpy_safe(temp_in_SWAP_99percent_unique, begin, element_size, element_size);
        memcpy_safe(begin, end, element_size, element_size);
        memcpy_safe(end, temp_in_SWAP_99percent_unique, element_size, element_size);
        begin += element_size;
        end -= element_size;
    }

}

void vec_cleanup(const void *v)
{
    struct vector_common_s *p_vec = *(struct vector_common_s * *)v;
    MY_UTILS_ASSERT(p_vec != NULL);
    p_vec->capacity = 0;
    p_vec->size = 0;
    my_free(p_vec->vector_buffer);
    p_vec->vector_buffer = NULL;
}

void vec_init(void *pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    p_vec->capacity = 0;
    p_vec->size = 0;
    p_vec->not_heap = 0;
    p_vec->ctn_type = container_type_vector;
    p_vec->vector_buffer = NULL;
}

void vec_destroy(void *pv_vec)
{
    struct vector_common_s *p_vec = (struct vector_common_s *)pv_vec;
    MY_UTILS_ASSERT(p_vec != NULL);
    p_vec->capacity = 0;
    p_vec->size = 0;
    my_free(p_vec->vector_buffer);
    p_vec->vector_buffer = NULL;
}
//================= vector 操作 end =======================

//================= string 操作 =======================


void string_assign_$(void *pv_string, void *new_string, enum string_e string_type)
{
    MY_UTILS_ASSERT(pv_string != NULL);
    MY_UTILS_ASSERT(new_string != NULL);

    struct string_s *p_src = (struct string_s *)pv_string;
    struct string_s *p_n = (struct string_s *)new_string;
    if(string_type == new_string_s_type){
        vec_resize(p_src, vec_size(p_n));
        memcpy_safe(&p_src->vector_buffer[0], &p_n->vector_buffer[0], string_len(p_n), string_len(p_n));
    }else{
        vec_resize(p_src, strlen(new_string) + 1);
        memcpy_safe(&p_src->vector_buffer[0], new_string, strlen(new_string), strlen(new_string));
    }

    vec_shrink_to_fit(p_src);
}



void string_append_$(void *pv_src, void *new_string, enum string_e string_type)
{
    MY_UTILS_ASSERT(pv_src != NULL);
    MY_UTILS_ASSERT(new_string != NULL);

    struct string_s *p_src = (struct string_s *)pv_src;
    struct string_s *p_n = (struct string_s *)new_string;

    size_t src_len_string_add;
    size_t add_len_string_add;
    size_t total_in_string_add;


    if(string_type == new_string_s_type){
        src_len_string_add = string_len(p_src);
        add_len_string_add = string_len(p_n);
        total_in_string_add = src_len_string_add + add_len_string_add + 1;

        vec_resize(p_src, total_in_string_add);
        memcpy_safe(vec_begin(p_src) + src_len_string_add, vec_begin(p_n), add_len_string_add, add_len_string_add);
    }else{
        src_len_string_add = string_len(p_src);
        add_len_string_add = strlen(new_string);
        total_in_string_add = src_len_string_add + add_len_string_add + 1;
        vec_resize(p_src, total_in_string_add);
        memcpy_safe(vec_begin(p_src) + src_len_string_add, new_string, add_len_string_add, add_len_string_add); \

    }
}

void string_erase_$(void *pv_string, size_t pos, size_t cnt)
{
    MY_UTILS_ASSERT(pv_string != NULL);

    string_t p_string = (string_t)pv_string;
    if(pos < vec_size(p_string) ){     /*开始删除的地方要在buffer中*/
        if(cnt >= vec_size(p_string) - pos){    // 删除的数量超出末尾, 直接修改size即可
            vec_resize(p_string, pos);           /*删除指定点后面的东西*/
            vec_push_back(p_string, '\0');      // 末尾填充'\0'
        }else{
            void *end_pos = &p_string->vector_buffer[pos + cnt];
            memmove(&p_string->vector_buffer[pos], end_pos, cnt);
            p_string->vector_buffer[pos + cnt] = '\0'; \
            vec_resize(p_string, vec_size(p_string) - cnt);
        }
    }

    vec_shrink_to_fit(p_string);
}

void string_insert_$(void *pv_string, size_t pos, void *new_string, enum string_e string_type)
{
    MY_UTILS_ASSERT(pv_string != NULL);
    MY_UTILS_ASSERT(new_string != NULL);
    struct string_s *p_src = (struct string_s *)pv_string;
    struct string_s *p_n = (struct string_s *)new_string;

    if(string_type == new_string_s_type){
        memcpy_safe(&p_src->vector_buffer[pos], &p_n->vector_buffer[0], string_len(p_n), string_len(p_n));
    }else{
        memcpy_safe(&p_src->vector_buffer[pos], new_string, strlen(new_string), strlen(new_string));
    }
}

int string_compare_$(void *pv_string, void *cmp_string, enum string_e string_type)
{
    MY_UTILS_ASSERT(pv_string != NULL);
    MY_UTILS_ASSERT(cmp_string != NULL);
    struct string_s *p_src = (struct string_s *)pv_string;
    struct string_s *p_cmp = (struct string_s *)cmp_string;
    int ret;
    if(string_type == new_string_s_type){
        ret = strcmp(&p_src->vector_buffer[0], &p_cmp->vector_buffer[0]);
    }else{
        ret = strcmp(&p_src->vector_buffer[0], cmp_string);
    }

    return ret;
}

//================= string 操作 end =======================

//================= deque 操作 ============================

size_t deq_size(void * pv_deq)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);

    return p_deq->size;
}

size_t deq_capacity(void * pv_deq)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    return p_deq->capacity;
}

#define DEQ_HEAD_RESERVED   8   // 头部预留空间
void deq_push_back_$(void *pv_deq, void *in_val)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    // 检测尾部是否超出范围
    bool tail_not_enough = p_deq->tail_idx >= (int)deq_capacity(p_deq) - DEQ_HEAD_RESERVED;
    if( tail_not_enough ){
        char *p_tmp = my_realloc(p_deq->deque_buffer, (deq_capacity(p_deq) + 16) * element_size );

        if (p_tmp == NULL){
            PT_LOG_CALLER_ADDR0();
            pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
        }

        p_deq->deque_buffer = p_tmp;
        p_deq->capacity += 16;

    }

    memcpy_safe(&p_deq->deque_buffer[(DEQ_HEAD_RESERVED + p_deq->tail_idx) * element_size], in_val, element_size, element_size);
    p_deq->size++;
    p_deq->tail_idx++;
}



void deq_pop_back_$(void *pv_deq, void *out_val)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    if(deq_size(p_deq) > 0){
        memcpy_safe(out_val, &p_deq->deque_buffer[(DEQ_HEAD_RESERVED + p_deq->tail_idx - 1) * element_size], element_size, element_size);
        p_deq->size--;
        p_deq->tail_idx--;
        return;
    }

    memset_safe(out_val, 0, element_size, element_size);    // 如果没有数据就擦除掉
}



void deq_push_front_$(void *pv_deq, void *in_val)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    // 检测头部是否超出范围
    bool head_not_enough = p_deq->head_idx < -DEQ_HEAD_RESERVED;
    if( head_not_enough || deq_capacity(p_deq) <= deq_size(p_deq) ){
        char *p_tmp = my_realloc(p_deq->deque_buffer, (deq_capacity(p_deq) + 16) * element_size );

        if (p_tmp == NULL){
            PT_LOG_CALLER_ADDR0();
            pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
        }

        p_deq->deque_buffer = p_tmp;
        p_deq->capacity += 16;
        char *p_mid = &p_deq->deque_buffer[DEQ_HEAD_RESERVED * element_size];
        memmove(&p_mid[0], &p_mid[-DEQ_HEAD_RESERVED * element_size], element_size * p_deq->size);
        p_deq->head_idx = -1;
        p_deq->tail_idx += DEQ_HEAD_RESERVED;

    }

    char *p_mid = &p_deq->deque_buffer[DEQ_HEAD_RESERVED * element_size];
    memcpy_safe(&p_mid[p_deq->head_idx * element_size], in_val, element_size, element_size);
    p_deq->size++;
    p_deq->head_idx--;
}



void deq_pop_front_$(void *pv_deq, void *out_val)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    if(deq_size(p_deq) > 0){
        char *p_mid = &p_deq->deque_buffer[DEQ_HEAD_RESERVED * element_size];
        memcpy_safe(out_val, &p_mid[(p_deq->head_idx + 1) * element_size], element_size, element_size);
        p_deq->size--;
        p_deq->head_idx++;
        return;
    }

    memset_safe(out_val, 0, element_size, element_size);    // 如果没有数据就擦除掉
}



void deq_clear(void *pv_deq)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    p_deq->size = 0;
    p_deq->head_idx = -1;
    p_deq->tail_idx = 0;
}

bool deq_empty(void *pv_deq)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);

    return (p_deq->size == 0);
}

void deq_resize_$(void *pv_deq, size_t new_size)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    // 如果new_size小于容量, 只要改size和尾部索引即可
    if(new_size <= p_deq->capacity){
        int delta = new_size - p_deq->size;
        p_deq->tail_idx -= delta;
        p_deq->size = new_size;

        return;
    }

    char *p_tmp = my_realloc(p_deq->deque_buffer, (new_size + DEQ_HEAD_RESERVED)  * element_size );
    if (p_tmp == NULL){
        PT_LOG_CALLER_ADDR0();
        pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
    }

    p_deq->deque_buffer = p_tmp;
    p_deq->capacity = new_size;

    p_deq->size = new_size;
    p_deq->tail_idx = p_deq->head_idx + new_size + 1;
}


void *deq_at_$(void *pv_deq, size_t idx)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    if (idx >= p_deq->size){
        PT_LOG_CALLER_ADDR0();
        pt_assert_handler(TO_STRING(idx >= p_deq->size), __FUNCTION__, __LINE__);
    }

    char *p_head = &p_deq->deque_buffer[(p_deq->head_idx + DEQ_HEAD_RESERVED + 1) * element_size];

    return &p_head[idx * element_size];

}

void deq_shrink_to_fit_$(void *pv_deq)
{
    struct deque_common_s *p_deq = (struct deque_common_s *)pv_deq;
    MY_UTILS_ASSERT(p_deq != NULL);
    size_t element_size = p_deq->element_size;

    if( deq_capacity(p_deq) > deq_size(p_deq) ){
        char *p_tmp;

        p_tmp = my_realloc(p_deq->deque_buffer, (p_deq->size + DEQ_HEAD_RESERVED) * element_size );
        MY_UTILS_ASSERT(p_tmp != NULL);

        if (p_tmp == NULL){
            PT_LOG_CALLER_ADDR0();
            pt_assert_handler(TO_STRING(p_tmp != NULL), __FUNCTION__, __LINE__);
        }

        char *p_head = &p_deq->deque_buffer[(p_deq->head_idx + 1) * element_size];
        char *p_mid = &p_deq->deque_buffer[DEQ_HEAD_RESERVED * element_size];
        memmove(&p_mid[0], p_head, element_size * p_deq->size);
        int delta = (-1) - p_deq->head_idx;
        p_deq->head_idx = -1;

        p_deq->tail_idx += delta;

        p_deq->deque_buffer = p_tmp;   // 缩小分配deqtor_buffer不会变的
        p_deq->capacity = p_deq->size;

    }
}


//================= deque 操作 end ========================

//================= find 操作 =============================

void *find_if_$(void *vbegin, void *vend, find_if_pre_t func, size_t element_size)
{
    char *begin = (char *)vbegin;
    char *end = (char *)vend;
    while(begin < end){
        if( func(begin) ){
            return (void *)begin;
        }
        begin += element_size;
    }

    begin = end;
    return (void *)begin;
}

void *find_$(void *vbegin, void *vend, void *target, size_t element_size)
{
    char *begin = (char *)vbegin;
    char *end = (char *)vend;
    while(begin < end){
        if(memcmp(begin, target, element_size) == 0){
            return (void *)begin;;
        }
        begin += element_size;
    }

    begin = end;
    return (void *)begin;
}

//================= find 操作 end =============================

//================= for_each 操作 =============================
void for_each_$(void *vbegin, void *vend, for_each_func_t   func, size_t element_size)
{
    if(func == NULL){
        return;
    }

    char *begin = (char *)vbegin;
    char *end = (char *)vend;
    for(; begin < end; begin += element_size){
        func(begin);
    }
}


//================= for_each 操作 end =============================

//================= 日期时间函数 =====================

static const char *short_mon[12] = {
  "Jan", "Feb", "Mar", "Apr", "May", "Jun",
  "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"
};
#include <stdlib.h>
// 限制长度的字符转换到整型数的函数
static int32_t n_atoi(const char *str, uint8_t max_len)
{
    char temp[12] = {0};
    if (max_len > 10){   // 10位十进制很大了
        max_len = 10;
    }

    memcpy(temp, str, max_len);

    return atoi(temp);
}
// 日期时间转换成tm格式
bool DATE_TIME_to_tm(struct tm *p_tm, char *date, char *time_)
{
    CHECK_LOCAL( p_tm != NULL, param_null);

    bool parse_date_res; parse_date_res = false;

    for (int i = 0; i < 12; i++){
        if ( strstr(date, short_mon[i]) != NULL ){
            p_tm->tm_mon = i;                       // 月从0算起
            parse_date_res = true;
        }
    }

    p_tm->tm_mday = n_atoi(date + 4, 2);

    p_tm->tm_year = n_atoi(date + 4 + 3, 4) - 1900; // tm_year从1900年算起

    p_tm->tm_hour = n_atoi(time_, 2);

    p_tm->tm_min = n_atoi(time_ + 3, 2);

    p_tm->tm_sec = n_atoi(time_ + 3 + 3, 2);

    return parse_date_res;

param_null:
    return false;
}
//================= 日期时间函数 end =====================

//================= CRC 操作 =============================
#ifdef USING_CRC
static const uint8_t auchCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40
};

static const uint8_t auchCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
0x40
};

uint16_t crc16_modbus(uint8_t *puchMsg, uint16_t usDataLen)
{
	uint8_t uchCRCHi = 0xFF;
	uint8_t uchCRCLo = 0xFF;
	uint32_t uIndex;
	while (usDataLen--)
	{
		uIndex = uchCRCHi ^ *puchMsg++;
		uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
		uchCRCLo = auchCRCLo[uIndex];
	}
	return (((uint16_t)uchCRCLo << 8u) | uchCRCHi);
}
#endif
//================= CRC 操作 end =============================

// CHECK错误处理函数示例
static void check_handler_default(const char *ex_string, int32_t alert_lvl, const char *func, uint32_t line)
{
    (void)ex_string;
    (void)alert_lvl;
    (void)func;
    (void)line;
    switch(alert_lvl){
        case RED_SEVERE:
            break;
        case ORANGE_HIGH:
            break;
        case YELLOW_ELEVATED:
            break;
        case BLUE_GUARDED:
            break;
        case GREEN_LOW:
            break;
        default:
            break;
        //
    }

}

check_handler_t     check_handler = check_handler_default;
void set_check_handler(check_handler_t  handler)
{
    if ( handler != (void *)0 ){
        check_handler = handler;
    }
}
